/*
* @File Name prm.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\realTimePrm\prm.c
* @Author: Ruige_Lee
* @Date:   2019-08-22 19:38:57
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-08-27 19:15:05
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/


#include <stdint.h>
#include <string.h>

#include <stdlib.h>

// #ifdef RAND_MAX
// #undef RAND_MAX
// #define RAND_MAX 32768
// #endif

#include "prm.h"




float samplePose[SAMPLETIMES][6];
float* routeIndex[2 * MAX_CHECKPOINT] = {NULL};



/**
 * 仅针对主机械臂0
 */
int32_t prm_execuse(float staPose[6], float endPose[6])
{
	int32_t status;

	prm_samplePose();

	status = prm_Astar_top( &staPose[0], &endPose[0], &routeIndex[0] );

	return status;
}


int32_t prm_direct_check( float staPose[6], float endPose[6], uint32_t step )
{
	uint32_t stepCnt = 0;

	if ( 0 != collide_check_top( &staPose[0] ) )
	{
		return -1;
	}

	if ( 0 != collide_check_top( &endPose[0] ) )
	{
		return -1;
	}


	float stepPose[6];
	float halfStepPose[6];
	float checkPose[6];

	for ( uint8_t axis = 0; axis < 6; axis ++ )
	{
		stepPose[axis] = endPose[axis] - staPose[axis];
	}

	uint8_t por = 0;
	while( stepCnt < step )
	{

		uint32_t div = fast_power(2,por);


		//每轮的第一个检查从起点开始用半步进错开
		for ( uint8_t axis = 0; axis < 6; axis ++ )
		{
			halfStepPose[axis] = stepPose[axis] / 2;
			checkPose[axis] = staPose[axis] + halfStepPose[axis];
		}
		if ( 0 != collide_check_top( &checkPose[0] ) )
		{
			return -1;
		}
		stepCnt++;

		//后续检查用全步进保持错位
		for ( uint32_t i = 0; i < (div-1); i++ )
		{
			for ( uint8_t axis = 0; axis < 6; axis ++ )
			{
				checkPose[axis] += stepPose[axis];
			}

			if ( 0 != collide_check_top( &checkPose[0] ) )
			{
				return -1;
			}
			stepCnt++;
		}

		for ( uint8_t axis = 0; axis < 6; axis ++ )
		{
			stepPose[axis] = halfStepPose[axis];
		}
		por++;
	}

	//步进值耗尽也没证明碰撞，即为安全
	return 0;

}


//全局可用的采样点，所以不做采样单点姿态的检测
int32_t prm_samplePose()
{

	for ( uint32_t i = 0; i < SAMPLETIMES; i++ )
	{
		float angle[6];

		angle[0] = -1.57 + rand() / 2147483647. * 3.14; // -90 ~ +90
		angle[1] = -1.57 + rand() / 2147483647. * 3.14; // -90 ~ +90
		angle[2] = -0.70 + rand() / 2147483647. * 4.5; // -40 ~ +220
		angle[3] = -1.57 + rand() / 2147483647. * 3.14; // -90 ~ +90
		angle[4] = -1.57 + rand() / 2147483647. * 3.14; // -90 ~ +90
		angle[5] = -1.57 + rand() / 2147483647. * 3.14; // -90 ~ +90


		samplePose[i][0] = angle[0];
		samplePose[i][1] = angle[1];
		samplePose[i][2] = angle[2];
		samplePose[i][3] = angle[3];
		samplePose[i][4] = angle[4];
		samplePose[i][5] = angle[5];

	}

	return 0;
}


